#include "ti_msp_dl_config.h"
#include "ti/driverlib/dl_i2c.h"
#include "MPU6050.h"  // 添加这一行，引用MPU6050.h
#include "MPU6050_Reg.h"

#include "arm_math.h"       // CMSIS-DSP库
#include <math.h>           // 数学函数
#include <stdlib.h>         // 提供abs函数


// 物理量转换系数 (根据您的量程配置)
#define GYRO_SCALE      (32.0f)     // LSB/(°/s) for ±500dps
#define ACCEL_SCALE     (8192.0f)   // LSB/g for ±4g
#define SAMPLE_RATE     100         // Hz
#define DT              (1.0f/SAMPLE_RATE)  // 0.01秒

// 漂移补偿参数
#define DRIFT_COMP_FACTOR  0.06f    // 提高漂移补偿系数
#define DRIFT_THRESHOLD    0.25f    // 降低静止状态阈值
#define STATIC_SAMPLES     25       // 减少静止样本数
#define MAX_ANGLE_JUMP     20.0f    // 最大允许角度突变阈值

// 卡尔曼滤波器参数 - 针对旋转精度优化
typedef struct {
    float Q_angle;   // 过程噪声协方差 - 角度的不确定性
    float Q_bias;    // 过程噪声协方差 - 角速度偏移的不确定性
    float R_measure; // 测量噪声协方差 - 加速度计测量的不确定性
    
    float angle;     // 角度
    float bias;      // 角速度偏移
    float rate;      // 角速度

    float P[2][2];   // 误差协方差矩阵
} Kalman_t;

// 全局卡尔曼滤波器实例
static Kalman_t kalmanPitch;
static Kalman_t kalmanRoll;
static Kalman_t kalmanYaw;

// 导出状态变量给main函数使用
bool isInMotionState = false;   // 保留此变量以兼容现有代码

// 四元数结构
typedef struct {
    float q0, q1, q2, q3;
} Quaternion;

// 全局变量
static Quaternion quat = {1.0f, 0.0f, 0.0f, 0.0f}; // 初始四元数
static float gravity[3] = {0.0f, 0.0f, 1.0f};       // 重力向量估计

// 传感器校准偏移
static float accelOffset[3] = {0};
static float gyroOffset[3] = {0};

// 静态变量用于记录上一次的角速度值
static float last_gx = 0.0f;
static float last_gy = 0.0f;
static float last_gz = 0.0f;

// 静态变量用于偏航角积分
static float yaw_integrated = 0.0f;

// 漂移补偿变量
static float drift_correction = 0.0f;
static int static_count = 0;
static bool was_rotating = false;
static float last_attitude[3] = {0.0f, 0.0f, 0.0f}; // 用于检测异常跳变

// 默认为水平安装
static MPU_MountOrientation currentMountOrientation = MPU_MOUNT_HORIZONTAL;

// 设置MPU6050安装方向
void MPU6050_SetMountOrientation(MPU_MountOrientation orientation) {
    currentMountOrientation = orientation;
    
    // 重置卡尔曼滤波器状态
    kalmanPitch.angle = 0.0f;
    kalmanPitch.bias = 0.0f;
    kalmanRoll.angle = 0.0f;
    kalmanRoll.bias = 0.0f;
    kalmanYaw.angle = 0.0f;
    kalmanYaw.bias = 0.0f;
    
    // 重置积分值
    yaw_integrated = 0.0f;
    
    // 重置漂移补偿变量
    drift_correction = 0.0f;
    static_count = 0;
    was_rotating = false;
    
    // 重置异常检测
    last_attitude[0] = 0.0f;
    last_attitude[1] = 0.0f;
    last_attitude[2] = 0.0f;
}

// 卡尔曼滤波器初始化
void Kalman_Init(Kalman_t *kalman) {
    // 提高初期响应性的参数
    kalman->Q_angle = 0.2f;     // 提高角度过程噪声
    kalman->Q_bias = 0.01f;     // 偏差过程噪声
    kalman->R_measure = 0.005f; // 降低测量噪声权重

    kalman->angle = 0.0f;      // 初始角度
    kalman->bias = 0.0f;       // 初始偏差
    kalman->rate = 0.0f;       // 初始角速度

    // 初始化误差协方差矩阵
    kalman->P[0][0] = 20.0f;
    kalman->P[0][1] = 0.0f;
    kalman->P[1][0] = 0.0f;
    kalman->P[1][1] = 20.0f;
}

// 卡尔曼滤波器计算函数 - 增强响应性版本
float Kalman_Update(Kalman_t *kalman, float newAngle, float newRate, float dt) {
    // 步骤1: 根据角速度和时间预测角度
    kalman->rate = newRate - kalman->bias;  // 角速度减去估计的偏差
    kalman->angle += dt * kalman->rate;     // 角度预测
    
    // 步骤2: 更新误差协方差矩阵 P
    // 使用更高的Q_angle加速收敛
    float dynamicQangle = kalman->Q_angle;
    
    // 如果角速度大，表示正在旋转，增大Q_angle以加速响应
    if (fabsf(newRate) > 1.0f) {  // 降低旋转检测阈值
        // 角速度越大，动态Q_angle越大，使快速旋转时响应更灵敏
        dynamicQangle = kalman->Q_angle * (25.0f + fabsf(newRate) * 1.0f); // 提高动态Q_angle
    }
    
    kalman->P[0][0] += dt * (dt * kalman->P[1][1] - kalman->P[0][1] - kalman->P[1][0] + dynamicQangle);
    kalman->P[0][1] -= dt * kalman->P[1][1];
    kalman->P[1][0] -= dt * kalman->P[1][1];
    kalman->P[1][1] += kalman->Q_bias * dt;
    
    // 步骤3: 计算卡尔曼增益
    float S = kalman->P[0][0] + kalman->R_measure;
    float K[2];
    K[0] = kalman->P[0][0] / S;
    K[1] = kalman->P[1][0] / S;
    
    // 步骤4: 使用测量值更新状态估计
    float y = newAngle - kalman->angle;
    
    // X轴朝上时的特殊残差处理
    if (currentMountOrientation == MPU_MOUNT_VERTICAL_X_UP) {
        // 残差非线性映射
        if (fabsf(y) > 0.5f && fabsf(y) < 5.0f) {
            // 中等残差区域增强响应
            y *= 1.25f;
        }
        else if (fabsf(y) >= 5.0f && fabsf(y) < 20.0f) {
            // 大残差区域控制响应
            y = (y > 0) ? 5.0f + (y - 5.0f) * 0.7f : -5.0f + (y + 5.0f) * 0.7f;
        }
    }

    // 残差上限保护
    float maxResidual = 45.0f;
    if (y > maxResidual) y = maxResidual;
    else if (y < -maxResidual) y = -maxResidual;
    
    kalman->angle += K[0] * y;
    kalman->bias += K[1] * y;
    
    // 步骤5: 更新误差协方差
    float P00_temp = kalman->P[0][0];
    float P01_temp = kalman->P[0][1];
    
    kalman->P[0][0] -= K[0] * P00_temp;
    kalman->P[0][1] -= K[0] * P01_temp;
    kalman->P[1][0] -= K[1] * P00_temp;
    kalman->P[1][1] -= K[1] * P01_temp;
    
    return kalman->angle;
}

// MPU6050的I2C从机地址 (7位地址，不含读写位)
#define MPU6050_ADDRESS		0x68

void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{

    uint8_t temp[2];
    temp[0] = RegAddress;
    temp[1] = Data;
 
    DL_I2C_fillControllerTXFIFO(I2C_0_INST, temp, 2);
    while (!(DL_I2C_getControllerStatus(I2C_0_INST) & DL_I2C_CONTROLLER_STATUS_IDLE));
 
    DL_I2C_startControllerTransfer(I2C_0_INST, MPU6050_ADDRESS, DL_I2C_CONTROLLER_DIRECTION_TX, 2);
    while (DL_I2C_getControllerStatus(I2C_0_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
    while (!(DL_I2C_getControllerStatus(I2C_0_INST) & DL_I2C_CONTROLLER_STATUS_IDLE));
    DL_I2C_flushControllerTXFIFO(I2C_0_INST);

}

uint8_t MPU6050_ReadReg(uint8_t RegAddress)
{
    
     uint8_t data;
 
    DL_I2C_fillControllerTXFIFO(I2C_0_INST, &RegAddress, 1);
    while (!(DL_I2C_getControllerStatus(I2C_0_INST) & DL_I2C_CONTROLLER_STATUS_IDLE));
    DL_I2C_startControllerTransfer(I2C_0_INST, MPU6050_ADDRESS, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
    while (DL_I2C_getControllerStatus(I2C_0_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
    while (!(DL_I2C_getControllerStatus(I2C_0_INST) & DL_I2C_CONTROLLER_STATUS_IDLE));
    DL_I2C_flushControllerTXFIFO(I2C_0_INST);
 
    DL_I2C_startControllerTransfer(I2C_0_INST, MPU6050_ADDRESS, DL_I2C_CONTROLLER_DIRECTION_RX, 1);
    while (DL_I2C_getControllerStatus(I2C_0_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
    while (!(DL_I2C_getControllerStatus(I2C_0_INST) & DL_I2C_CONTROLLER_STATUS_IDLE));
    data = DL_I2C_receiveControllerData(I2C_0_INST);
 
    return data;

}

void MPU6050_Init(void)
{
    
    MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01);        // 电源管理寄存器1，取消睡眠模式，选择时钟源为X轴陀螺仪
    MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);        // 电源管理寄存器2，保持默认值0，所有轴均不待机
    MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09);        // 采样率分频寄存器，配置采样率
    MPU6050_WriteReg(MPU6050_CONFIG, 0x06);            // 配置寄存器，配置DLPF
    MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x08);       // 陀螺仪配置寄存器，选择满量程为±500°/s
    MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x08);      // 加速度计配置寄存器，选择满量程为±4g
}
void MPU6050_GetData(int16_t *AccX, int16_t *AccY, int16_t *AccZ, 
                      int16_t *GyroX, int16_t *GyroY, int16_t *GyroZ)
{
    uint8_t DataH, DataL;                            // 定义数据高8位和低8位的变量
    
    DataH = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H);    // 读取加速度计X轴的高8位数据
    DataL = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L);    // 读取加速度计X轴的低8位数据
    *AccX = (DataH << 8) | DataL;                    // 数据拼接，通过输出参数返回
    
    DataH = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H);    // 读取加速度计Y轴的高8位数据
    DataL = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L);    // 读取加速度计Y轴的低8位数据
    *AccY = (DataH << 8) | DataL;                    // 数据拼接，通过输出参数返回
    
    DataH = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H);    // 读取加速度计Z轴的高8位数据
    DataL = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L);    // 读取加速度计Z轴的低8位数据
    *AccZ = (DataH << 8) | DataL;                    // 数据拼接，通过输出参数返回
    
    DataH = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H);    // 读取陀螺仪X轴的高8位数据
    DataL = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L);    // 读取陀螺仪X轴的低8位数据
    *GyroX = (DataH << 8) | DataL;                    // 数据拼接，通过输出参数返回
    
    DataH = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H);    // 读取陀螺仪Y轴的高8位数据
    DataL = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L);    // 读取陀螺仪Y轴的低8位数据
    *GyroY = (DataH << 8) | DataL;                    // 数据拼接，通过输出参数返回
    
    DataH = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H);    // 读取陀螺仪Z轴的高8位数据
    DataL = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L);    // 读取陀螺仪Z轴的低8位数据
    *GyroZ = (DataH << 8) | DataL;                    // 数据拼接，通过输出参数返回
}




/* 四元数转欧拉角函数 (保留以便其他地方可能需要) */
void QuaternionToEuler(Attitude *att) {
    // 使用重力向量计算俯仰和横滚
    att->roll = atan2f(gravity[1], gravity[2]);
    att->pitch = atan2f(-gravity[0], sqrtf(gravity[1]*gravity[1] + 
                                          gravity[2]*gravity[2]));
    
    // 计算偏航角 (使用四元数)
    float siny = 2.0f * (quat.q0 * quat.q3 + quat.q1 * quat.q2);
    float cosy = quat.q0*quat.q0 + quat.q1*quat.q1 - 
                 quat.q2*quat.q2 - quat.q3*quat.q3;
    att->yaw = atan2f(siny, cosy);
    
    // 转换为角度
    att->pitch *= 180.0f / (float)M_PI;
    att->roll *= 180.0f / (float)M_PI;
    att->yaw *= 180.0f / (float)M_PI;
}

// 注意：使用系统提供的DL_Common_delayCycles函数
// 不再自定义delay_cycles函数

/* 传感器校准函数 */
void CalibrateSensors(int16_t *AccX, int16_t *AccY, int16_t *AccZ, 
                     int16_t *GyroX, int16_t *GyroY, int16_t *GyroZ, 
                     int samples) {
    int32_t accelSum[3] = {0}, gyroSum[3] = {0};
    
    for (int i = 0; i < samples; i++) {
        // 获取传感器数据 (假设通过中断或轮询获取)
        // 这里需要您添加获取数据的实际方法
        // 例如: MPU6050_ReadData(AccX, AccY, AccZ, GyroX, GyroY, GyroZ);
        
        accelSum[0] += *AccX;
        accelSum[1] += *AccY;
        accelSum[2] += *AccZ;
        
        gyroSum[0] += *GyroX;
        gyroSum[1] += *GyroY;
        gyroSum[2] += *GyroZ;
        
        // 等待下一个采样 (10ms对应100Hz)
        // 使用系统函数而非自定义函数
        DL_Common_delayCycles(120000); // 大约10ms延迟 @ 48MHz
    }
    
    // 计算偏移量
    accelOffset[0] = (float)accelSum[0] / samples;
    accelOffset[1] = (float)accelSum[1] / samples;
    accelOffset[2] = (float)accelSum[2] / samples - ACCEL_SCALE; // Z轴减去1g
    
    gyroOffset[0] = (float)gyroSum[0] / samples;
    gyroOffset[1] = (float)gyroSum[1] / samples;
    gyroOffset[2] = (float)gyroSum[2] / samples;
}

/* 姿态解算主函数 - 支持不同安装方向 */
void UpdateAttitude(int16_t AccX, int16_t AccY, int16_t AccZ, 
                   int16_t GyroX, int16_t GyroY, int16_t GyroZ, 
                   Attitude *attitude) {
    // 静态变量，确保卡尔曼滤波器只初始化一次
    static bool isKalmanInitialized = false;
    
    // 初始化卡尔曼滤波器
    if (!isKalmanInitialized) {
        Kalman_Init(&kalmanPitch);
        Kalman_Init(&kalmanRoll);
        Kalman_Init(&kalmanYaw); // 初始化新的Yaw滤波器
        isKalmanInitialized = true;
        
        // 强制初始角度为零
        kalmanPitch.angle = 0.0f;
        kalmanRoll.angle = 0.0f;
        kalmanYaw.angle = 0.0f;
        
        // 初始化上一次的角速度值
        last_gx = 0.0f;
        last_gy = 0.0f;
        last_gz = 0.0f;
        
        // 初始化异常检测
        last_attitude[0] = 0.0f;
        last_attitude[1] = 0.0f;
        last_attitude[2] = 0.0f;
    }
    
    // 简单的初始校准（只在第一次调用时执行）
    static bool isCalibrated = false;
    static int calibrationCount = 0;
    // 预热计数器，确保传感器先稳定工作
    static int warmupCount = 0;
    
    // 硬编码初始偏移量 - 可以通过实际测量调整
    static bool isOffsetInitialized = false;
    if (!isOffsetInitialized) {
        // 设置预估的偏移量，取代长时间校准
        accelOffset[0] = 0.0f;  // 根据实际传感器调整
        accelOffset[1] = 0.0f;
        accelOffset[2] = 0.0f;
        
        gyroOffset[0] = 0.0f;   // 根据实际传感器调整
        gyroOffset[1] = 0.0f;
        gyroOffset[2] = 0.0f;
        
        isOffsetInitialized = true;
    }
    
    if (!isCalibrated) {
        // 直接跳过预热阶段，使用默认值
        warmupCount = 10;
        
        // 直接使用当前数据快速校准
        // 仅收集15个样本，增加校准样本数提高精度
        if (calibrationCount < 15) {
            // 累加传感器数据
            accelOffset[0] += AccX;
            accelOffset[1] += AccY;
            accelOffset[2] += AccZ - (int16_t)ACCEL_SCALE; // Z轴减去1g
            
            gyroOffset[0] += GyroX;
            gyroOffset[1] += GyroY;
            gyroOffset[2] += GyroZ;
            
            calibrationCount++;
            
            // 校准期间强制角度为零
            attitude->pitch = 0.0f;
            attitude->roll = 0.0f;
            attitude->yaw = 0.0f;
            return;
        } else {
            // 计算平均偏移
            accelOffset[0] /= 15.0f; // 调整为15个样本
            accelOffset[1] /= 15.0f;
            accelOffset[2] /= 15.0f;
            
            gyroOffset[0] /= 15.0f;
            gyroOffset[1] /= 15.0f;
            gyroOffset[2] /= 15.0f;
            
            // 重置卡尔曼滤波器状态
            kalmanPitch.angle = 0.0f;
            kalmanPitch.bias = 0.0f;
            kalmanRoll.angle = 0.0f;
            kalmanRoll.bias = 0.0f;
            kalmanYaw.angle = 0.0f;
            kalmanYaw.bias = 0.0f;
            
            // 重置偏航角积分
            yaw_integrated = 0.0f;
            
            // 重置漂移补偿变量
            drift_correction = 0.0f;
            static_count = 0;
            was_rotating = false;
            
            // 初始化异常检测
            last_attitude[0] = 0.0f;
            last_attitude[1] = 0.0f;
            last_attitude[2] = 0.0f;
            
            isCalibrated = true;
        }
    }
    
    // 应用校准偏移并转换为物理单位
    float ax = (AccX - accelOffset[0]) / ACCEL_SCALE;
    float ay = (AccY - accelOffset[1]) / ACCEL_SCALE;
    float az = (AccZ - accelOffset[2]) / ACCEL_SCALE;
    
    float gx = (GyroX - gyroOffset[0]) / GYRO_SCALE; // 角速度，度/秒
    float gy = (GyroY - gyroOffset[1]) / GYRO_SCALE;
    float gz = (GyroZ - gyroOffset[2]) / GYRO_SCALE;
    
    // 检测异常数据 - 如果角速度突然过大，可能是传感器异常
    if (fabsf(gx) > 500.0f || fabsf(gy) > 500.0f || fabsf(gz) > 500.0f) {
        // 异常数据，忽略本次更新
        return;
    }
    
    // 应用陀螺仪数据平滑 - 减少噪声但保持快速响应
    // 使用指数移动平均，快速变化时权重更高
    float alpha_gyro = 0.9f; // 默认平滑系数，增大以提高响应速度
    
    // 检测快速变化，减少平滑效果
    if (fabsf(gx - last_gx) > 1.0f) alpha_gyro = 0.99f; // 提高角速度阈值敏感度
    if (fabsf(gy - last_gy) > 1.0f) alpha_gyro = 0.99f;
    if (fabsf(gz - last_gz) > 1.0f) alpha_gyro = 0.99f;
    
    // 应用平滑
    float smooth_gx = alpha_gyro * gx + (1.0f - alpha_gyro) * last_gx;
    float smooth_gy = alpha_gyro * gy + (1.0f - alpha_gyro) * last_gy;
    float smooth_gz = alpha_gyro * gz + (1.0f - alpha_gyro) * last_gz;
    
    // 保存当前值作为下一次的历史值
    last_gx = gx;
    last_gy = gy;
    last_gz = gz;
    
    // 根据安装方向调整计算方法
    float roll_acc, pitch_acc;
    float yaw_rate;
    
    if (currentMountOrientation == MPU_MOUNT_VERTICAL_Y_UP) {
        // 竖直安装，Y轴朝上
        // 重新映射轴：X->X, Y->Z, Z->-Y
        
        // 计算欧拉角
        roll_acc = atan2f(ax, sqrtf(ay*ay + az*az)) * 180.0f / (float)M_PI;
        pitch_acc = atan2f(az, sqrtf(ax*ax + ay*ay)) * 180.0f / (float)M_PI;
        
        // 使用Y轴角速度作为偏航参考
        yaw_rate = smooth_gy; // Y轴角速度作为偏航角速度
    } 
    else if (currentMountOrientation == MPU_MOUNT_VERTICAL_X_UP) {
        // 竖直安装，X轴朝上
        // 重新映射轴：X->Z, Y->Y, Z->-X
        
        // 计算欧拉角
        roll_acc = atan2f(ay, sqrtf(ax*ax + az*az)) * 180.0f / (float)M_PI;
        pitch_acc = atan2f(-az, sqrtf(ay*ay + ax*ax)) * 180.0f / (float)M_PI;
        
        // 优化偏航角计算，结合多轴信息
        yaw_rate = smooth_gx * 1.15f; // 提高X轴角速度权重
        
        // 使用组合方式计算yaw_rate以提高精度
        if (fabsf(smooth_gy) > 2.0f || fabsf(smooth_gz) > 2.0f) {
            // 快速旋转时考虑其他轴信息
            float aux_rate = (smooth_gy * 0.2f + smooth_gz * 0.2f);
            yaw_rate = (yaw_rate * 0.8f) + aux_rate;
        }
    }
    else {
        // 默认水平安装，Z轴朝上
        roll_acc = atan2f(ay, sqrtf(ax*ax + az*az)) * 180.0f / (float)M_PI;
        pitch_acc = atan2f(-ax, sqrtf(ay*ay + az*az)) * 180.0f / (float)M_PI;
        
        // 使用Z轴角速度作为偏航参考
        yaw_rate = smooth_gz;
    }
    
    // 增强型漂移补偿 - 检测静止状态
    bool is_static = (fabsf(smooth_gx) < DRIFT_THRESHOLD && 
                      fabsf(smooth_gy) < DRIFT_THRESHOLD && 
                      fabsf(smooth_gz) < DRIFT_THRESHOLD);
    
    // 检测是否刚从旋转状态变为静止状态
    if (is_static && was_rotating) {
        static_count = 0;
        was_rotating = false;
    } else if (!is_static) {
        was_rotating = true;
        static_count = 0;
    }
    
    // 在静止状态下累积静止样本计数
    if (is_static) {
        static_count++;
        if (static_count > STATIC_SAMPLES) {
            // 在长时间静止后，缓慢修正漂移
            drift_correction = -yaw_integrated * DRIFT_COMP_FACTOR;
        }
    } else {
        // 旋转时不应用漂移补偿
        drift_correction = 0.0f;
    }
    
    // 检测明显旋转，使用更大的积分系数
    float yaw_scale = 2.0f; // 默认积分系数
    if (fabsf(yaw_rate) > 1.5f) { // 旋转阈值
        // 角速度越大，积分系数越大，非线性映射
        yaw_scale = 2.0f + 0.5f * fabsf(yaw_rate) / 10.0f; // 最大可达约2.5
    }
    
    // 应用积分和漂移补偿
    yaw_integrated += yaw_rate * DT * yaw_scale + drift_correction * DT;
    
    // 更新卡尔曼滤波器
    float new_pitch, new_roll;
    
    if (currentMountOrientation == MPU_MOUNT_VERTICAL_Y_UP) {
        // 竖直安装，Y轴朝上，使用重新映射的角速度
        new_roll = Kalman_Update(&kalmanRoll, roll_acc, smooth_gx, DT);
        new_pitch = Kalman_Update(&kalmanPitch, pitch_acc, smooth_gz, DT);
    } 
    else if (currentMountOrientation == MPU_MOUNT_VERTICAL_X_UP) {
        // 竖直安装，X轴朝上，使用重新映射的角速度
        new_roll = Kalman_Update(&kalmanRoll, roll_acc, smooth_gy, DT);
        new_pitch = Kalman_Update(&kalmanPitch, pitch_acc, smooth_gz, DT);
    }
    else {
        // 水平安装
        new_pitch = Kalman_Update(&kalmanPitch, pitch_acc, smooth_gy, DT);
        new_roll = Kalman_Update(&kalmanRoll, roll_acc, smooth_gx, DT);
    }
    
    // 对于偏航角，直接使用卡尔曼滤波器追踪角速度积分
    // 将积分值作为测量值，角速度作为预测值
    float new_yaw = Kalman_Update(&kalmanYaw, yaw_integrated, yaw_rate, DT);
    
    // 限制偏航角在-180到180度范围内
    if (new_yaw > 180.0f) new_yaw -= 360.0f;
    else if (new_yaw < -180.0f) new_yaw += 360.0f;
    
    // 检测异常角度跳变 - 防止突然变化
    float pitch_delta = fabsf(new_pitch - last_attitude[0]);
    float roll_delta = fabsf(new_roll - last_attitude[1]);
    float yaw_delta = fabsf(new_yaw - last_attitude[2]);
    
    // 如果静止状态下出现大角度跳变，认为是异常，忽略本次更新
    if (is_static && (pitch_delta > MAX_ANGLE_JUMP || 
                     roll_delta > MAX_ANGLE_JUMP || 
                     yaw_delta > MAX_ANGLE_JUMP)) {
        // 异常跳变，保持上一次的角度值
        new_pitch = last_attitude[0];
        new_roll = last_attitude[1];
        new_yaw = last_attitude[2];
    }
    
    // 应用自适应低通滤波器平滑输出，但保持快速响应
    // 快速变化时使用更小的滤波系数，使响应更快
    float alpha_pitch = 0.95f; // 默认滤波系数
    float alpha_roll = 0.95f;
    float alpha_yaw = 0.95f;
    
    // 如果角度变化大，减小滤波系数，使响应更快
    // 使用非线性映射，变化越大，滤波越少
    if (pitch_delta > 0.5f) alpha_pitch = 0.95f + 0.03f * (pitch_delta / 5.0f); // 最大可达0.98
    if (roll_delta > 0.5f) alpha_roll = 0.95f + 0.03f * (roll_delta / 5.0f);
    if (yaw_delta > 0.5f) alpha_yaw = 0.95f + 0.03f * (yaw_delta / 5.0f);
    
    // 限制最大滤波系数
    if (alpha_pitch > 0.99f) alpha_pitch = 0.99f;
    if (alpha_roll > 0.99f) alpha_roll = 0.99f;
    if (alpha_yaw > 0.99f) alpha_yaw = 0.99f;
    
    // 应用滤波
    attitude->pitch = alpha_pitch * new_pitch + (1.0f - alpha_pitch) * attitude->pitch;
    attitude->roll = alpha_roll * new_roll + (1.0f - alpha_roll) * attitude->roll;
    attitude->yaw = alpha_yaw * new_yaw + (1.0f - alpha_yaw) * attitude->yaw;
    
    // 保存当前角度作为下一次的历史值
    last_attitude[0] = attitude->pitch;
    last_attitude[1] = attitude->roll;
    last_attitude[2] = attitude->yaw;
    
    // 更新重力向量（用于其他计算，比如四元数）
    if (currentMountOrientation == MPU_MOUNT_VERTICAL_Y_UP) {
        // 竖直安装，Y轴朝上
        gravity[0] = sinf(attitude->roll * (float)M_PI / 180.0f);
        gravity[1] = cosf(attitude->roll * (float)M_PI / 180.0f) * 
                      cosf(attitude->pitch * (float)M_PI / 180.0f);
        gravity[2] = -cosf(attitude->roll * (float)M_PI / 180.0f) * 
                      sinf(attitude->pitch * (float)M_PI / 180.0f);
    } 
    else if (currentMountOrientation == MPU_MOUNT_VERTICAL_X_UP) {
        // 竖直安装，X轴朝上
        gravity[0] = cosf(attitude->roll * (float)M_PI / 180.0f) * 
                     cosf(attitude->pitch * (float)M_PI / 180.0f);
        gravity[1] = sinf(attitude->roll * (float)M_PI / 180.0f);
        gravity[2] = -cosf(attitude->roll * (float)M_PI / 180.0f) * 
                     sinf(attitude->pitch * (float)M_PI / 180.0f);
    }
    else {
        // 水平安装，Z轴朝上
        gravity[0] = sinf(attitude->pitch * (float)M_PI / 180.0f);
        gravity[1] = -sinf(attitude->roll * (float)M_PI / 180.0f) * 
                      cosf(attitude->pitch * (float)M_PI / 180.0f);
        gravity[2] = -cosf(attitude->roll * (float)M_PI / 180.0f) * 
                      cosf(attitude->pitch * (float)M_PI / 180.0f);
    }
                  
    // 状态标志，保持与之前代码兼容
    isInMotionState = !is_static;

    // 在UpdateAttitude函数中添加，针对偏航角进行特殊处理
    if (currentMountOrientation == MPU_MOUNT_VERTICAL_X_UP) {
        // 检测旋转结束回归静止状态
        if (!is_static && was_rotating && fabsf(yaw_rate) < 0.8f) {
            // 应用额外补偿抵消滞回效应
            drift_correction = -yaw_integrated * 0.01f;
        }
    }

    // 增强漂移检测与补偿
    if (currentMountOrientation == MPU_MOUNT_VERTICAL_X_UP) {
        static float angle_history[10] = {0};
        static int history_index = 0;
        
        // 记录历史角度值
        angle_history[history_index] = attitude->yaw;
        history_index = (history_index + 1) % 10;
        
        // 检测长时间小范围波动（零漂特征）
        if (static_count > 15) {
            float max_diff = 0;
            for (int i = 0; i < 10; i++) {
                for (int j = i+1; j < 10; j++) {
                    float diff = fabsf(angle_history[i] - angle_history[j]);
                    if (diff > max_diff) max_diff = diff;
                }
            }
            
            // 小范围波动时施加智能补偿
            if (max_diff < 1.2f && max_diff > 0.05f) {
                // 计算均值偏差并应用补偿
                float sum = 0;
                for (int i = 0; i < 10; i++) {
                    sum += angle_history[i];
                }
                float avg = sum / 10.0f;
                
                // 自适应强度漂移补偿
                drift_correction = (yaw_integrated - avg * 0.1f) * DRIFT_COMP_FACTOR * 1.5f;
            }
        }
        
        // 旋转停止回零优化
        if (was_rotating && is_static && static_count < 10) {
            // 应用温和回零力
            attitude->yaw = attitude->yaw * 0.97f + yaw_integrated * 0.03f;
        }
    }

    // 在UpdateAttitude函数中修改偏航角处理
    float yaw_integrated_old = yaw_integrated;

    // 自适应偏航角积分算法
    if (currentMountOrientation == MPU_MOUNT_VERTICAL_X_UP) {
        // 快速响应模式
        if (fabsf(yaw_rate) > 1.5f) {
            // 非线性增益映射，角速度越大增益越大
            float adaptive_gain = 1.0f + powf(fabsf(yaw_rate)/10.0f, 0.7f);
            adaptive_gain = fminf(adaptive_gain, 2.8f); // 最大增益限制
            
            yaw_integrated += yaw_rate * DT * adaptive_gain;
        } 
        // 精细调整模式
        else if (fabsf(yaw_rate) > 0.3f) {
            yaw_integrated += yaw_rate * DT * 1.2f;
        }
        // 静止修正模式
        else {
            // 增强修正力度
            yaw_integrated += yaw_rate * DT + drift_correction * DT * 2.0f;
        }
        
        // 角度突变检测与补偿
        if (fabsf(yaw_integrated - yaw_integrated_old) > 5.0f && is_static) {
            yaw_integrated = yaw_integrated_old + (yaw_integrated - yaw_integrated_old) * 0.1f;
        }
    }

    // X轴朝上时的优化轴处理和融合
    if (currentMountOrientation == MPU_MOUNT_VERTICAL_X_UP) {
        // 优化的欧拉角计算
        roll_acc = atan2f(ay, sqrtf(ax*ax + az*az)) * 180.0f / (float)M_PI;
        pitch_acc = atan2f(-az, sqrtf(ay*ay + ax*ax)) * 180.0f / (float)M_PI;
        
        // 高精度多轴融合策略
        float primary_rate = smooth_gx * 1.25f;
        
        // 动态权重多轴融合
        float gy_weight = 0.0f, gz_weight = 0.0f;
        
        // 基于旋转状态动态调整权重
        if (fabsf(smooth_gx) > 2.0f) {
            gy_weight = 0.12f * (fabsf(smooth_gy)/fabsf(smooth_gx + 0.01f));
            gz_weight = 0.12f * (fabsf(smooth_gz)/fabsf(smooth_gx + 0.01f));
        } else {
            gy_weight = 0.05f;
            gz_weight = 0.05f;
        }
        
        // 权重上限
        if (gy_weight > 0.25f) gy_weight = 0.25f;
        if (gz_weight > 0.25f) gz_weight = 0.25f;
        
        // 组合角速度
        yaw_rate = primary_rate + smooth_gy * gy_weight + smooth_gz * gz_weight;
    }
}

